/**
 * Copyright (c) 2010-2011 Morten Silcowitz
 *
 * This file is part of jinngine.
 *
 * jinngine is free software: you can redistribute it and/or modify it
 * under the terms of its license which may be found in the accompanying
 * LICENSE file or <http://code.google.com/p/jinngine/>.
 */
package space.math;

import java.io.Serializable;
import java.util.Collection;
import jinngine.math.Matrix3;
import jinngine.math.Vector3;

/**
 * <code>Vector3d</code> defines a Vector for a three float value tuple.
 * <code>Vector3d</code> can represent any three dimensional value, such as a
 * vertex or normal.
 * 
 * The functional methods like add, sub, multiply that returns new instances,
 * and left <code>this</code> unchanged.
 * 
 * Static methods store the resulting vector on a existing reference, which
 * avoid allocation an can improve performances around 20% (profiling performed
 * on vector addition).
 * 
 * Depreciated methods will be removed on October 2010
 * 
 * @author Morten Silcowitz
 * @author Pierre Labatut
 */
public final class Vector3f implements Serializable {

    private static final long serialVersionUID = 1L;
    /**
     * The x coordinate.
     */
    public float x;
    /**
     * The y coordinate.
     */
    public float y;
    /**
     * The z coordinate.
     */
    public float z;

    // public transient final static float e = 1e-9f;
    /**
     * Constructs and initialises a <code>Vector3</code> to [0., 0., 0.]
     */
    public Vector3f() {
        x = 0;
        y = 0;
        z = 0;
    }

    /**
     * Constructs and initialises a <code>Vector3</code> from the specified xyz
     * coordinates.
     * 
     * @param x
     *            the x coordinate
     * @param y
     *            the y coordinate
     * @param z
     *            the z coordinate
     */
    public Vector3f(final float x, final float y, final float z) {
        this.x = x;
        this.y = y;
        this.z = z;
        assert isValid();
    }

    /**
     * Constructs and initialises a <code>Vector3</code> with the coordinates of
     * the given <code>Vector3</code>.
     * 
     * @param v
     *            the <code>Vector3</code> containing the initialisation x y z
     *            data
     * @throws NullPointerException
     *             when v is null
     */
    public Vector3f(final Vector3f v) {
        x = v.x;
        y = v.y;
        z = v.z;
        assert isValid();
    }

    /**
     * Create a new unit vector heading positive x
     * 
     * @return a new unit vector heading positive x
     */
    public static Vector3f i() {
        return new Vector3f(1.f, 0.f, 0.f);
    }

    /**
     * Create a new unit vector heading positive y
     * 
     * @return a new unit vector heading positive y
     */
    public static Vector3f j() {
        return new Vector3f(0.f, 1.f, 0.f);
    }

    /**
     * Create a new unit vector heading positive z
     * 
     * @return a new unit vector heading positive z
     */
    public static Vector3f k() {
        return new Vector3f(0.f, 0.f, 1.f);
    }

    /**
     * Adds a provided vector to this vector creating a resultant vector which
     * is returned. Neither <code>this</code> nor <code>v</code> is modified.
     * 
     * @param v
     *            the vector to add to this.
     * @return resultant vector
     * @throws NullPointerException
     *             if v is null
     */
    public final Vector3f add(final Vector3f v) {
        return new Vector3f(x + v.x, y + v.y, z + v.z);
    }

    /**
     * Multiply the vector coordinates by -1. creating a resultant vector which
     * is returned. <code>this</code> vector is not modified.
     * 
     * @return resultant vector
     * @throws NullPointerException
     *             if v is null
     */
    public final Vector3f negate() {
        return new Vector3f(-x, -y, -z);
    }

    /**
     * Returns true if one of the coordinated is not a number <code>this</code>
     * vector is not modified.
     * 
     * @return true if one of the coordinated is not a number
     */
    public final boolean isNaN() {
        return Float.isNaN(x) || Float.isNaN(y) || Float.isNaN(z);
    }

    /**
     * Get a coordinate from a dimension ordinal.
     * 
     * @param i
     *            the dimension ordinal number. 1 is x, 2 is y 3 is z.
     * @return <ul>
     *         <li>x coordinate when i is 0</li>
     *         <li>y coordinate when i is 1</li>
     *         <li>z coordinate when i is 2</li>
     *         </ul>
     */
    public float get(final int i) {
        return i > 0 ? (i > 1 ? z : y) : x;
    }

    /**
     * Set a coordinate from a dimension ordinal.
     * 
     * @param i
     *            the dimension ordinal number. 1 is x, 2 is y 3 is z.
     * @param v
     *            new coordinate value
     */
    public void set(final int i, final float v) {
        if (i == 0) {
            x = v;
        } else {
            if (i == 1) {
                y = v;
            } else {
                z = v;
            }
        }
    }

    /**
     * Add two vectors and place the result in v1. <code>v2</code> is not
     * modified.
     * 
     * @param v1
     *            a not null reference, store the sum
     * @param v2
     *            a not null reference
     * @throws NullPointerException
     *             if v1 or v2 is null
     */
    public static void add(final Vector3f v1, final Vector3f v2) {
        v1.x += v2.x;
        v1.y += v2.y;
        v1.z += v2.z;
    }

    /**
     * Subtract two vectors and place the result in v1. <code>v2</code> is not
     * modified.
     * 
     * @param v1
     *            a not null reference, store the difference
     * @param v2
     *            a not null reference
     * @throws NullPointerException
     *             if v1 or v2 is null
     */
    public static void sub(final Vector3f v1, final Vector3f v2) {
        v1.x -= v2.x;
        v1.y -= v2.y;
        v1.z -= v2.z;
    }

    /**
     * Subtracts a provided vector to this vector creating a resultant vector
     * which is returned. Neither <code>this</code> nor <code>v</code> is
     * modified.
     * 
     * @param v
     *            the vector to add to this.
     * @return resultant vector
     */
    public final Vector3f sub(final Vector3f v) {
        return new Vector3f(x - v.x, y - v.y, z - v.z);
    }

    /**
     * Subtracts a provided vector to this vector. <code>this</code> contains
     * the result and <code>v</code> is not modified.
     * 
     * @param v
     *            vector to substract
     * @return <code>this</code>
     */
    public final Vector3f assignSub(final Vector3f v) {
        x -= v.x;
        y -= v.y;
        z -= v.z;
        return this;
    }

    public final Vector3f assignAdd(final Vector3f v) {
        x += v.x;
        y += v.y;
        z += v.z;
        assert isValid();
        return this;
    }

    public final Vector3f assignAdd(final float q, final float w, final float e) {
        x += q;
        y += w;
        z += e;
        return this;
    }

    /**
     * Multiply this vector by the scalar s
     * 
     * @param s
     *            scalar value
     * @return this vector
     */
    public final Vector3f assignMultiply(final float s) {
        x *= s;
        y *= s;
        z *= s;
        return this;
    }

    /**
     * Assign p+q to this vector
     */
    public final Vector3f assignSum(final Vector3f p, final Vector3f q) {
        x = p.x + q.x;
        y = p.y + q.y;
        z = p.z + q.z;
        return this;
    }

    /**
     * Multiply this vector by a provided scalar creating a resultant vector
     * which is returned. <code>this</code> vector is not modified.
     * 
     * @param s
     *            multiplication coefficient
     * @return resultant vector
     */
    public final Vector3f multiply(final float s) {
        return new Vector3f(x * s, y * s, z * s);
    }

    /**
     * Scale vector by the scale matrix given by s. <code>this</code> vector is
     * not modified.
     * 
     * @param s
     *            scale direction and factor
     * @return an new vector
     */
    public final Vector3f scale(final Vector3f s) {
        return new Vector3f(x * s.x, y * s.y, z * s.z);
    }

    /**
     * Return the norm of a-b
     */
    public static final float normOfDifference(final Vector3f a, final Vector3f b) {
        return (float) Math.sqrt(
                (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z));
    }

    /**
     * Return the XY norm of a-b, disregarding the value of z
     */
    public static final float xynormOfDifference(final Vector3f a, final Vector3f b) {
        return (float) Math.sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
    }

    /**
     * Return the squared norm of a-b
     */
    public static final float squaredNormOfDifference(final Vector3f a, final Vector3f b) {
        return (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z);
    }

    /**
     * Return the norm of a+b
     */
    public static final float normOfSum(final Vector3f a, final Vector3f b) {
        return (float) Math.sqrt(
                (a.x + b.x) * (a.x + b.x) + (a.y + b.y) * (a.y + b.y) + (a.z + b.z) * (a.z + b.z));
    }

    /**
     * Return the squared norm of a+b
     */
    public static final float squaredNormOfSum(final Vector3f a, final Vector3f b) {
        return (a.x + b.x) * (a.x + b.x) + (a.y + b.y) * (a.y + b.y) + (a.z + b.z) * (a.z + b.z);
    }

    /**
     * Multiply v by s and place the result in v, so v = v s
     */
    public final static void multiply(final Vector3f v, final float s) {
        v.x *= s;
        v.y *= s;
        v.z *= s;
    }

    /**
     * Multiply v by s and add the result to r, such that r = r + v s
     */
    public final static void multiplyAndAdd(final Vector3f v, final float s, final Vector3f r) {
        r.x += v.x * s;
        r.y += v.y * s;
        r.z += v.z * s;
    }

    /**
     * Multiply Mv by s and add the result to r, such that r = r + Mv s
     */
    public final static void multiplyAndAdd(final Matrix3 A, final Vector3f v, final float s, final Vector3f r) {
        r.x += (v.x * A.a11 + v.y * A.a12 + v.z * A.a13) * s;
        r.y += (v.x * A.a21 + v.y * A.a22 + v.z * A.a23) * s;
        r.z += (v.x * A.a31 + v.y * A.a32 + v.z * A.a33) * s;
    }

    /**
     * r = r + Av
     */
    public final static void transformAndAdd(final Matrix3 A, final Vector3f v, final Vector3f r) {
        r.x += v.x * A.a11 + v.y * A.a12 + v.z * A.a13;
        r.y += v.x * A.a21 + v.y * A.a22 + v.z * A.a23;
        r.z += v.x * A.a31 + v.y * A.a32 + v.z * A.a33;
    }

    /**
     * Multiply v by s, and store result in v. Add v to result and store in
     * result
     * 
     * @param v
     * @param s
     * @param result
     * @throws NullPointerException
     *             if v ot result is null
     */
    public static void multiplyStoreAndAdd(final Vector3f v, final float s, final Vector3f result) {
        v.x *= s;
        v.y *= s;
        v.z *= s;
        result.x += v.x;
        result.y += v.y;
        result.z += v.z;
    }

    /**
     * Return the scalar value of (y1-y2)^Ty3
     */
    public static final float subAndDot(final Vector3f y1, final Vector3f y2, final Vector3f y3) {
        return (y1.x - y2.x) * y3.x + (y1.y - y2.y) * y3.y + (y1.z - y2.z) * y3.z;
    }

    /**
     * Returns the dot product of this vector and vector v. Neither
     * <code>this</code> nor <code>v</code> is modified.
     * 
     * @param v
     *            the other vector
     * @return the dot product of this and v1
     * @throws NullPointerException
     *             if v is null
     */
    public final float dot(final Vector3f v) {
        return x * v.x + y * v.y + z * v.z;
    }

    /**
     * Returns the dot product of this vector and vector v. Neither
     * <code>this</code> nor <code>v</code> is modified. z coordinated if
     * truncated
     * 
     * @param v
     *            the other vector
     * @return the dot product of this and v1
     * @throws NullPointerException
     */
    public final float xydot(final Vector3f v) {
        return x * v.x + y * v.y;
    }

    /**
     * Return a new new set to the cross product of this vectors and v Neither
     * <code>this</code> nor <code>v</code> is modified.
     * 
     * @param v
     *            a not null vector
     * @return the cross product
     * @throws NullPointerException
     *             when v is null
     */
    public final Vector3f cross(final Vector3f v) {
        return new Vector3f(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
    }

    /**
     * Compute the cross product between this vector and the given vector v.
     * Place result in this vector
     */
    public final Vector3f assignCross(final Vector3f v) {
        final float tmpx = y * v.z - z * v.y;
        final float tmpy = z * v.x - x * v.z;
        final float tmpz = x * v.y - y * v.x;
        x = tmpx;
        y = tmpy;
        z = tmpz;
        return this;
    }

    /**
     * Sets result vector to the vector cross product of vectors v1 and v2.
     * Neither <code>v1</code> nor <code>v2</code> is modified.
     * 
     * @param v1
     *            the first vector
     * @param v2
     *            the second vector
     * @param result
     */
    public static void crossProduct(final Vector3f v1, final Vector3f v2, final Vector3f result) {
        final float tempa1 = v1.y * v2.z - v1.z * v2.y;
        final float tempa2 = v1.z * v2.x - v1.x * v2.z;
        final float tempa3 = v1.x * v2.y - v1.y * v2.x;

        result.x = tempa1;
        result.y = tempa2;
        result.z = tempa3;
    }

    /**
     * Return a new vector set to the normalisation of vector v1.
     * <code>this</code> vector is not modified.
     * 
     * @return the normalised vector
     */
    public final Vector3f normalize() {
        return new Vector3f(this).assignNormalize();
        //        float l = Math.sqrt(x * x + y * y + z * z);
        //        if (l == 0.0) {
        //            return new Vector3(1, 0, 0);
        //        }
        //        l = 1. / l;
        //        return new Vector3(x * l, y * l, z * l);
    }

    /**
     * Sets the value of this <code>Vector3</code> to the specified x, y and
     * coordinates.
     * 
     * @param x
     *            the x coordinate
     * @param y
     *            the y coordinate
     * @param z
     *            the z coordinate
     * @return return this
     */
    public final Vector3f assign(final float x, final float y, final float z) {
        this.x = x;
        this.y = y;
        this.z = z;
        return this;
    }

    /**
     * A this vector to the provided coordinates creating a new resultant
     * vector. <code>this</code> vector is not modified
     * 
     * @param x
     *            the x coordinate
     * @param y
     *            the y coordinate
     * @param z
     *            the z coordinate
     * @return the result vector
     */
    public final Vector3f add(final float x, final float y, final float z) {
        return new Vector3f(this.x + x, this.y + y, this.z + z);
    }

    /**
     * Sets the value of this vector to the value of the xyz coordinates of the
     * given vector. <code>v</code> is not modified
     * 
     * @param v
     *            the vector to be copied
     * @return <code>this</code>
     * @throws NullPointerException
     */
    public final Vector3f assign(final Vector3f v) {
        final float t1 = v.x;
        final float t2 = v.y;
        final float t3 = v.z;
        x = t1;
        y = t2;
        z = t3;
        return this;
    }

    /**
     * Assign the zero vector to this vector
     */
    public final Vector3f assignZero() {
        x = 0;
        y = 0;
        z = 0;
        return this;
    }

    /**
     * Apply scaling to this vector
     * 
     * @param x
     * @param y
     * @param z
     * @return
     */
    public final Vector3f assignScale(final float xs, final float ys, final float zs) {
        x *= xs;
        y *= ys;
        z *= zs;
        return this;
    }

    /**
     * Assign the difference of a and b to this vector
     * 
     * @param a
     * @param b
     * @return
     */
    public final Vector3f assignDifference(final Vector3f a, final Vector3f b) {
        x = a.x - b.x;
        y = a.y - b.y;
        z = a.z - b.z;
        return this;
    }

    /**
     * Negate the value of this vector
     */
    public final Vector3f assignNegate() {
        x *= -1;
        y *= -1;
        z *= -1;
        return this;
    }

    /**
     * Normalise this vector
     */
    public final Vector3f assignNormalize() {
        final float n = norm();
        assert n > 1e-14;
        final float s = 1.0f / n;
        x *= s;
        y *= s;
        z *= s;
        return this;
    }

    /**
     * Multiply v by s, and add the result to this vector
     */
    public final Vector3f assignAddProduct(final Vector3f v, final float s) {
        x += v.x * s;
        y += v.y * s;
        z += v.z * s;
        return this;
    }

    /**
     * Returns the length of this vector. <code>this</code> vector is not
     * modified.
     * 
     * @return Returns the length of this vector.
     */
    public final float norm() {
        return (float) Math.sqrt(x * x + y * y + z * z);
    }

    /**
     * Returns the length of this vector. z coordinate is truncated.
     * <code>this</code> vector is not modified.
     * 
     * @return float.NaN when float.isNaN(x) || float.isNaN(y)
     */
    public final float xynorm() {
        return (float) Math.sqrt(x * x + y * y);
    }

    /**
     * Returns the length of this vector. <code>this</code> vector is not
     * modified.
     * 
     * @return the length of this vector
     */
    public final float squaredNorm() {
        return x * x + y * y + z * z;
    }

    /**
     * Returns the length of this vector, in the xy plane. <code>this</code>
     * vector is not modified.
     * 
     * @return the length of this vector
     */
    public final float squaredXYNorm() {
        return x * x + y * y;
    }

    /**
     * Return the infinity norm, absolute value of the largest entry.
     * 
     * @return infinity norm of this vector
     */
    public final float infnorm() {
        return x > y ? (x > z ? x : z) : y > z ? y : z;
    }

    /**
     * Returns <tt>true</tt> if the absolute value of the three coordinates are
     * smaller or equal to epsilon.
     * 
     * @param epsilon
     *            positive tolerance around zero
     * @return true when the coordinates are next to zero false in the other
     *         cases
     */
    public final boolean isEpsilon(final float epsilon) {
        if (epsilon < 0.) {
            throw new IllegalArgumentException("epsilon must be positive");
        }
        return -epsilon <= x && x <= epsilon && -epsilon <= y && y <= epsilon && -epsilon <= z && z <= epsilon;
    }

    /**
     * Pack the three coordinates into a new float array <code>this</code>
     * vector is not modified.
     * 
     * @return a array set with x, y and z
     */
    public final float[] toArray() {
        return new float[]{x, y, z};
    }

    /**
     * Build an array of {@link float} from a collection of {@link Vector3}. A
     * new array is allocated with a length equal to
     * <code>vectors.size()*3</code> The given collection is not modified.
     * 
     * @param vectors
     *            a not null collection ov vector
     * @return a array of packed vector coordinates
     */
    public static float[] toArray(final Collection<Vector3f> vectors) {
        // convert points
        final float[] array = new float[3 * vectors.size()];
        int i = 0;
        for (final Vector3f v : vectors) {
            array[i++] = v.x;
            array[i++] = v.y;
            array[i++] = v.z;
        }
        return array;
    }

    

    /**
     * Returns a string representation of this vector. The string representation
     * consists of the three dimensions in the order x, y, z, enclosed in square
     * brackets (<tt>"[]"</tt>). Adjacent elements are separated by the
     * characters <tt>", "</tt> (comma and space). Elements are converted to
     * strings as by {@link float#toString(float)}.
     * 
     * @return a string representation of this vector
     */
    @Override
    public final String toString() {
        return "[" + x + ", " + y + ", " + z + "]";
    }

    /**
     * Return this vector as a formated string, that can be directly used in a
     * MATLAB script.
     */
    public final String toMatlabString() {
        return "[" + x + "; " + y + "; " + z + "]";
    }

    public boolean isValid(){
        return Float.isNaN(x)==false && Float.isNaN(y)==false && Float.isNaN(z)==false;
    }
}
